/******************************************************************************
 * Copyright 2022 The AIR Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "v2x-asn-msgs-internal.h"

bool asn_priority_set(OCTET_STRING_t *prio, int32_t value) {
  if (!prio) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  uint8_t data = value;
  data = data << 5;
  OCTET_STRING_fromBuf(prio, (const char *)&data, 1);
  return true;
}

int32_t asn_priority_get(const OCTET_STRING_t *prio) {
  if (!prio) {
    return 0;
  }

  if (!prio->buf) {
    return 0;
  }

  uint8_t value = *(prio->buf);
  return value >> 5;
}

bool rsi_refpath_asn2pb(const Position3D_t *ref_position,
                        const ReferencePath_t *ref_path_asn,
                        ::v2xpb::asn::RsiReferencePath *ref_path_pb) {
  if (!ref_position || !ref_path_asn || !ref_path_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  for (int i = 0; i < ref_path_asn->activePath.list.count; i++) {
    if (!position_asn2pb(ref_position, ref_path_asn->activePath.list.array[i],
                         ref_path_pb->add_points())) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }
  ref_path_pb->set_radius(static_cast<double>(ref_path_asn->pathRadius) / 10);
  return true;
}

bool rsi_refpath_pb2asn(const Position3D_t *ref_position,
                        const ::v2xpb::asn::RsiReferencePath &ref_path_pb,
                        ReferencePath_t *ref_path_asn) {
  if (!ref_position || !ref_path_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  for (const auto &point : ref_path_pb.points()) {
    ASN_SEQUENCE_ADD(&ref_path_asn->activePath.list,
                     ASN1(PositionOffsetLLV)::create_tp());
    if (!position_pb2asn(ref_position, point,
                         ref_path_asn->activePath.list
                             .array[ref_path_asn->activePath.list.count - 1])) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }
  ref_path_asn->pathRadius = static_cast<int>(round(ref_path_pb.radius() * 10));
  return true;
}

bool rsi_rte_asn2pb(const Position3D_t *ref_position, const RTEData_t *rte_asn,
                    ::v2xpb::asn::RsiRte *rte_pb) {
  if (!ref_position || !rte_asn || !rte_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  rte_pb->set_rte_id(int32_t(rte_asn->rteId));
  rte_pb->set_event_type(int32_t(rte_asn->eventType));
  rte_pb->set_event_source([](EventSource_t src) {
    switch (src) {
      case EventSource_police:
        return v2xpb::asn::RsiRte_Source_SRC_POLICE;
      case EventSource_government:
        return v2xpb::asn::RsiRte_Source_SRC_GOVERNMENT;
      case EventSource_meteorological:
        return v2xpb::asn::RsiRte_Source_SRC_METEOROLOGICAL;
      case EventSource_internet:
        return v2xpb::asn::RsiRte_Source_SRC_INTERNET;
      case EventSource_detection:
        return v2xpb::asn::RsiRte_Source_SRC_DETECTION;
      default:
        return v2xpb::asn::RsiRte_Source_SRC_UNKNOWN;
    }
  }(rte_asn->eventSource));
  if (nullptr != rte_asn->eventPos) {
    if (!position_asn2pb(ref_position, rte_asn->eventPos,
                         rte_pb->mutable_position())) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }
  if (nullptr != rte_asn->eventRadius) {
    rte_pb->set_event_radius(static_cast<double>(*rte_asn->eventRadius) / 10);
  }
  if (nullptr != rte_asn->description) {
    if (rte_asn->description->present == Description_PR_textString) {
      rte_pb->set_description(
          (const char *)rte_asn->description->choice.textString.buf,
          rte_asn->description->choice.textString.size);
    } else if (rte_asn->description->present == Description_PR_textGB2312) {
      rte_pb->set_description(
          (const char *)rte_asn->description->choice.textGB2312.buf,
          rte_asn->description->choice.textGB2312.size);
    }
  }
  if (nullptr != rte_asn->priority) {
    // rte_pb->set_priority((const char *) rte_asn->priority->buf,
    // rte_asn->priority->size);
    rte_pb->set_priority(asn_priority_get(rte_asn->priority));
  }
  if (nullptr != rte_asn->referencePaths) {
    for (int i = 0; i < rte_asn->referencePaths->list.count; i++) {
      if (!rsi_refpath_asn2pb(ref_position,
                              rte_asn->referencePaths->list.array[i],
                              rte_pb->add_ref_paths())) {
        LOG(ERROR) << "Failed to convert reference path.";
        return false;
      }
    }
  }
  if (nullptr != rte_asn->eventConfidence) {
    auto conf = *rte_asn->eventConfidence;
    rte_pb->set_event_confidence(
        0.005 * static_cast<double>(conf));  // 0~200, unit:0.005
  }
  return true;
}
bool rsi_rts_asn2pb(const Position3D_t *ref_position, const RTSData_t *rts_asn,
                    ::v2xpb::asn::RsiRts *rts_pb) {
  if (!ref_position || !rts_asn || !rts_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  rts_pb->set_rts_id(int32_t(rts_asn->rtsId));
  rts_pb->set_sign_type(int32_t(rts_asn->signType));
  if (nullptr != rts_asn->signPos) {
    if (!position_asn2pb(ref_position, rts_asn->signPos,
                         rts_pb->mutable_position())) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }
  if (nullptr != rts_asn->description) {
    if (rts_asn->description->present == Description_PR_textString) {
      rts_pb->set_description(
          (const char *)rts_asn->description->choice.textString.buf,
          rts_asn->description->choice.textString.size);
    } else if (rts_asn->description->present == Description_PR_textGB2312) {
      rts_pb->set_description(
          (const char *)rts_asn->description->choice.textGB2312.buf,
          rts_asn->description->choice.textGB2312.size);
    }
  }
  if (nullptr != rts_asn->priority) {
    // rts_pb->set_priority((const char *) rts_asn->priority->buf,
    // rts_asn->priority->size);
    rts_pb->set_priority(asn_priority_get(rts_asn->priority));
  }
  if (nullptr != rts_asn->referencePaths) {
    for (int i = 0; i < rts_asn->referencePaths->list.count; i++) {
      if (!rsi_refpath_asn2pb(ref_position,
                              rts_asn->referencePaths->list.array[i],
                              rts_pb->add_ref_paths())) {
        LOG(ERROR) << "Failed to convert reference path.";
        return false;
      }
    }
  }
  return true;
}

bool rsi_rte_pb2asn(const Position3D_t *ref_position,
                    const ::v2xpb::asn::RsiRte &rte_pb, RTEData_t *rte_asn) {
  if (!ref_position || !rte_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  rte_asn->rteId = rte_pb.rte_id();
  rte_asn->eventType = rte_pb.event_type();

  rte_asn->eventSource = [](::v2xpb::asn::RsiRte_Source src) {
    switch (src) {
      case v2xpb::asn::RsiRte_Source_SRC_POLICE:
        return EventSource_police;
      case v2xpb::asn::RsiRte_Source_SRC_GOVERNMENT:
        return EventSource_government;
      case v2xpb::asn::RsiRte_Source_SRC_METEOROLOGICAL:
        return EventSource_meteorological;
      case v2xpb::asn::RsiRte_Source_SRC_INTERNET:
        return EventSource_internet;
      case v2xpb::asn::RsiRte_Source_SRC_DETECTION:
        return EventSource_detection;
      default:
        return EventSource_unknown;
    }
  }(rte_pb.event_source());
  if (rte_pb.has_position()) {
    rte_asn->eventPos = ASN1(PositionOffsetLLV)::create_tp();
    if (!position_pb2asn(ref_position, rte_pb.position(), rte_asn->eventPos)) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }
  if (rte_pb.has_event_radius()) {
    rte_asn->eventRadius = ASN1(Radius)::create_tp();
    *rte_asn->eventRadius = (int32_t)round(rte_pb.event_radius() * 10);
  }
  if (rte_pb.has_description()) {
    rte_asn->description = ASN1(Description)::create_tp();
    rte_asn->description->present = Description_PR_textString;
    OCTET_STRING_fromBuf(&rte_asn->description->choice.textString,
                         rte_pb.description().c_str(),
                         static_cast<int>(rte_pb.description().length()));
  }
  if (rte_pb.has_priority()) {
    rte_asn->priority = ASN1(RSIPriority)::create_tp();
    // OCTET_STRING_fromBuf(rte_asn->priority, rte_pb.priority().c_str(), (int)
    // rte_pb.priority().length());
    asn_priority_set(rte_asn->priority, rte_pb.priority());
  }
  for (const auto &ref_path_pb : rte_pb.ref_paths()) {
    ASN1(ReferencePathList)::create_tp(&rte_asn->referencePaths);
    ASN_SEQUENCE_ADD(&rte_asn->referencePaths->list,
                     ASN1(ReferencePath)::create_tp());
    if (!rsi_refpath_pb2asn(
            ref_position, ref_path_pb,
            rte_asn->referencePaths->list
                .array[rte_asn->referencePaths->list.count - 1])) {
      LOG(ERROR) << "Failed to convert reference path.";
      return false;
    }
  }
  if (rte_pb.has_event_confidence()) {
    rte_asn->eventConfidence = ASN1(Confidence)::create_tp();
    auto conf = rte_pb.event_confidence();
    *rte_asn->eventConfidence = static_cast<int64_t>(conf * 200);
  }
  return true;
}
bool rsi_rts_pb2asn(const Position3D_t *ref_position,
                    const ::v2xpb::asn::RsiRts &rts_pb, RTSData_t *rts_asn) {
  if (!ref_position || !rts_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  rts_asn->rtsId = rts_pb.rts_id();
  rts_asn->signType = rts_pb.sign_type();
  if (rts_pb.has_position()) {
    rts_asn->signPos = ASN1(PositionOffsetLLV)::create_tp();
    if (!position_pb2asn(ref_position, rts_pb.position(), rts_asn->signPos)) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }
  if (rts_pb.has_description()) {
    rts_asn->description = ASN1(Description)::create_tp();
    rts_asn->description->present = Description_PR_textString;
    OCTET_STRING_fromBuf(&rts_asn->description->choice.textString,
                         rts_pb.description().c_str(),
                         static_cast<int>(rts_pb.description().length()));
  }
  if (rts_pb.has_priority()) {
    rts_asn->priority = ASN1(RSIPriority)::create_tp();
    // OCTET_STRING_fromBuf(rts_asn->priority, rts_pb.priority().c_str(), (int)
    // rts_pb.priority().length());
    asn_priority_set(rts_asn->priority, rts_pb.priority());
  }
  for (const auto &ref_path_pb : rts_pb.ref_paths()) {
    ASN1(ReferencePathList)::create_tp(&rts_asn->referencePaths);
    ASN_SEQUENCE_ADD(&rts_asn->referencePaths->list,
                     ASN1(ReferencePath)::create_tp());
    if (!rsi_refpath_pb2asn(
            ref_position, ref_path_pb,
            rts_asn->referencePaths->list
                .array[rts_asn->referencePaths->list.count - 1])) {
      LOG(ERROR) << "Failed to convert reference path.";
      return false;
    }
  }
  return true;
}

bool rsi_asn2pb(const RoadSideInformation_t *rsi_asn,
                ::v2xpb::asn::Rsi *rsi_pb) {
  if (!rsi_asn || !rsi_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  rsi_pb->set_message_count(int32_t(rsi_asn->msgCnt));
  rsi_pb->set_rsi_id((const char *)rsi_asn->id.buf, rsi_asn->id.size);
  // Position
  if (!position_asn2pb(&rsi_asn->refPos, rsi_pb->mutable_position())) {
    LOG(ERROR) << "Failed to convert position.";
    return false;
  }
  if (nullptr != rsi_asn->rtes) {
    for (int i = 0; i < rsi_asn->rtes->list.count; i++) {
      if (!rsi_rte_asn2pb(&rsi_asn->refPos, rsi_asn->rtes->list.array[i],
                          rsi_pb->add_rtes())) {
        LOG(ERROR) << "Failed to convert rte";
        return false;
      }
    }
  }
  if (nullptr != rsi_asn->rtss) {
    for (int i = 0; i < rsi_asn->rtss->list.count; i++) {
      if (!rsi_rts_asn2pb(&rsi_asn->refPos, rsi_asn->rtss->list.array[i],
                          rsi_pb->add_rtss())) {
        LOG(ERROR) << "Failed to convert rte";
        return false;
      }
    }
  }
  return true;
}

bool rsi_pb2asn(const ::v2xpb::asn::Rsi &rsi_pb,
                RoadSideInformation_t *rsi_asn) {
  if (!rsi_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  rsi_asn->msgCnt = rsi_pb.message_count() & 127;
  OCTET_STRING_fromBuf(&rsi_asn->id, rsi_pb.rsi_id().c_str(),
                       rsi_pb.rsi_id().length());
  // Position
  if (!position_pb2asn(rsi_pb.position(), &rsi_asn->refPos)) {
    LOG(ERROR) << "Failed to convert position.";
    return false;
  }
  for (const auto &rte_pb : rsi_pb.rtes()) {
    ASN1(RTEList)::create_tp(&rsi_asn->rtes);
    ASN_SEQUENCE_ADD(&rsi_asn->rtes->list, ASN1(RTEData)::create_tp());
    if (!rsi_rte_pb2asn(
            &rsi_asn->refPos, rte_pb,
            rsi_asn->rtes->list.array[rsi_asn->rtes->list.count - 1])) {
      LOG(ERROR) << "Failed to convert rte";
      return false;
    }
  }
  for (const auto &rts_pb : rsi_pb.rtss()) {
    ASN1(RTSList)::create_tp(&rsi_asn->rtss);
    ASN_SEQUENCE_ADD(&rsi_asn->rtss->list, ASN1(RTSData)::create_tp());
    if (!rsi_rts_pb2asn(
            &rsi_asn->refPos, rts_pb,
            rsi_asn->rtss->list.array[rsi_asn->rtss->list.count - 1])) {
      LOG(ERROR) << "Failed to convert rts";
      return false;
    }
  }
  return true;
}
